from CV_interface.temporal_cv import TemporalCVSeq, TemporalCV
from FingerPrint_interface.temporal_fingerprint import TemporalFingerprintSeq, TemporalFingerprint
from ParticleFilter.Particle import Particle, w_gauss, WeightedDistribution
from FingerPrint_interface.fingerprint_interface import FingerprintDatabase, Fingerprint
from PIL import Image
import random
import math
import time
import matplotlib.pyplot as plt
#
PARTICLE_COUNT = 50    # Total number of particles
DELTA_TIMESTAMP = 400
MAX_TOLERATE_TIME_DELTA = 2000
MAX_TOLERATE_PHYSICAL_DELTA = 2000
EPSILON = 0.0

FP_D_211 = 5
FP_D_KEJIYUAN = 5
FP_D_OFFICE = 5

CV_D_211 = 200
CV_D_COR = 200
CV_D_KEJIYUAN = 200
CV_D_OFFICE = 200

STEP_D_211 = 150
STEP_D_COR = 600
STEP_D_KEJIYUAN = 270
STEP_D_OFFICE = 300


def distance(x1, y1, x2, y2):
    return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)


def find_next(stepYaw, timestamp):
    if len(stepYaw) == 0:
        return [0, 0, 0]
    st = 0
    en = len(stepYaw) - 1
    if (stepYaw[0][0] >= timestamp):
        return stepYaw[0]
    if (stepYaw[en][0] < timestamp):
        return stepYaw[en]
    while (st + 1 < en):
        mid = int((st + en) / 2)
        if (stepYaw[mid][0] >= timestamp):
            en = mid
        else:
            st = mid
    return stepYaw[en]


def rssi_distance_211(d):
    # d0=1, rssi = -50
    a = 0.48
    d0 = -50
    return d0-a*10*math.log(d, 10)


def particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw_list, fp_database, place):
    stx, sty, enx, eny = 0, 0, 0, 0
    deviation = []

    rect = []
    Beacons_locations = []
    # map
    if place == '11-211':
        """
        stx, sty, enx, eny = 0, -500, 800, 1000
        rect = [(200, -400, 30, 250), (570, -400, 30, 250), (230, -300, 570, 50), (200, 75, 30, 250),
                (570, 75, 30, 250), (230, 175, 570, 50), (200, 550, 30, 250), (570, 550, 30, 250), (230, 650, 570, 50)]
        Beacons_locations = [(150, 1000), (150, -500), (enx, 400), (0, 200), (enx, -400), (enx, 900)]
        """
        stx, sty, enx, eny = 0, -1000, 800, 2000
        rect = [(200, -800, 30, 500), (570, -800, 30, 500), (230, -600, 570, 100), (200, 150, 30, 500),
                (570, 150, 30, 500), (230, 350, 570, 100), (200, 1100, 30, 500), (570, 1100, 30, 500), (230, 1300, 570, 100)]
        Beacons_locations = [(150, 2000), (150, -1000), (enx, 800), (0, 400), (enx, -800), (enx, 1800)]
        FP_D = FP_D_211
        CV_D = CV_D_211
        STEP_D = STEP_D_211

    if place == 'cooridor':
        stx, sty, enx, eny = 0, 0, 160, 4320
        rect = [(-30, -30, 30, 4380), (-30, -30, 220, 30), (160, -30, 30, 4380), (-30, 4320, 220, 30)]
        Beacons_locations = [(0, 3740), (160, 3040), (160, 120)]
        FP_D = FP_D_211
        CV_D = CV_D_COR
        STEP_D = STEP_D_COR

    if place == 'kejiyuan':
        img = Image.open('kejiyuan_map.jpg')
        stx, sty, enx, eny = 0, 0, 1269,  1117
        rect = [(600, 350, 500, 150), (600, 750, 500, 150), (550, 150, 600, 30), (450, 150, 30, 300)]
        Beacons_locations = [(700, 550), (1100, 250), (380, 400), (800, -50), (900, 550), (900, 150), (350, 950), (1100, 550), (700, 750), (900, 750), (900, 1050), (350, 220), (350, 550), (1100, 0), (350, 750), (1100, 750), (700, 1050), (500, -50), (1100, 950), (700, 150)]
        FP_D = FP_D_KEJIYUAN
        CV_D = CV_D_KEJIYUAN
        STEP_D = STEP_D_KEJIYUAN

    if place == 'office':
        img = Image.open('office_map.JPG')
        stx, sty, enx, eny = 0, 0, 648, 915
        rect = []
        Beacons_locations = [(210, 190), ()]
        FP_D = FP_D_OFFICE
        CV_D = CV_D_OFFICE
        STEP_D = STEP_D_OFFICE

    if place == 'new211':
        img = Image.open('map_211_3.png')
        stx, sty, enx, eny = 0, 0, 664, 1125
        rect = [(219, 83, 55, 270), (450, 83, 110,270), (270, 166, 380, 110), (219, 425, 55, 270), (450, 427, 110,270), (270, 500, 380, 110), (219, 767, 55, 270), (450, 770, 110,270), (270, 850, 380, 110)]
        Beacons_locations = [(63, 836), (205, 221), (50, 223), (634, 780)]
        FP_D = FP_D_211
        CV_D = CV_D_211
        STEP_D = STEP_D_211

    # init
    counter = 0
    particles = [Particle(x=random.uniform(stx, enx), y=random.uniform(sty, eny)) for _ in range(0, PARTICLE_COUNT)]

    tem_cv_index = 0
    print(len(tem_cv_seq.TemporalCV_list), len(stepYaw_list), len(tem_fp_seq.tem_fp))
    now_timestamp = tem_cv_seq.TemporalCV_list[0].timestamp
    if len(stepYaw_list) > 0:
        now_timestamp = max(now_timestamp, stepYaw_list[0][0])
    if len(tem_fp_seq.tem_fp) > 0:
        now_timestamp = max(now_timestamp, tem_fp_seq.tem_fp[0].timestamp)
    #now_timestamp = max(tem_cv_seq.TemporalCV_list[0].timestamp, stepYaw_list[0][0], tem_fp_seq.tem_fp[0].timestamp)
    num_cv = len(tem_cv_seq.TemporalCV_list)
    last_stepYaw = -1000
    TimeConsuming = []
    while tem_cv_index + 1 < num_cv:
        startTime = time.time()

        # read sensor
        while (tem_cv_index + 1 < num_cv) and (now_timestamp >= tem_cv_seq.TemporalCV_list[tem_cv_index].timestamp):
            tem_cv_index += 1
        now_cv_points = tem_cv_seq.TemporalCV_list[tem_cv_index].locations
        now_fp = tem_fp_seq.find_next(now_timestamp)
        now_fingerprint = tem_fp_seq.find_all(now_timestamp, MAX_TOLERATE_TIME_DELTA)
        now_stepYaw = find_next(stepYaw_list, now_timestamp)
        stepLength = STEP_D * now_stepYaw[2]
        if last_stepYaw != -1000:
            for p in particles:
                p.w *= w_gauss(float(distance(m_x, m_y, p.x, p.y)) / 10000, 0)

        print(counter, now_timestamp, tem_cv_seq.TemporalCV_list[tem_cv_index].timestamp, now_cv_points, tem_cv_index, now_fp, now_fingerprint, now_stepYaw)
        if math.fabs(tem_cv_seq.TemporalCV_list[tem_cv_index].timestamp - now_timestamp) < MAX_TOLERATE_TIME_DELTA:
            # cv data legal
            for p in particles:
                d = 10000000
                for (x, y) in now_cv_points:
                    d = min(d, distance(x, y, p.x, p.y))
                p.w *= w_gauss(float(d) / CV_D, 0)

        d_h = 0
        if math.fabs(tem_cv_seq.TemporalCV_list[tem_cv_index].timestamp - now_timestamp) < MAX_TOLERATE_TIME_DELTA:
            #stepYaw data legal
            if (last_stepYaw != -1000):
                d_h = (now_stepYaw[1] - last_stepYaw) * 180.0 / math.pi
            last_stepYaw = now_stepYaw[1]

        l = len(now_fingerprint)
        for i in range(l):
            if now_fingerprint[i] is not None:
                for p in particles:
                    d = 0
                    if place == '11-211':
                        d = rssi_distance_211(distance(
                            Beacons_locations[i][0],
                            Beacons_locations[i][1], p.x, p.y)) - now_fingerprint[i]
                    if place == 'cooridor':
                        d = rssi_distance_211(distance(
                            Beacons_locations[i][0],
                            Beacons_locations[i][1], p.x, p.y)) - now_fingerprint[i]
                    p.w *= w_gauss(float(d) / FP_D, 0)

        """
        if math.fabs(now_fp.timestamp - now_timestamp) < MAX_TOLERATE_TIME_DELTA:
            # fingerprint data legal
            for p in particles:
                if place == '11-211':
                    d = rssi_distance_211(distance(
                        Beacons_locations[now_fp.index][0],
                        Beacons_locations[now_fp.index][1], p.x, p.y)) - now_fp.rssi
                else:
                    tmp = []
                    minn_dis = 10000
                    nearest = 0
                    for fp in fp_database.fingerprints:
                        dis = distance(fp.x, fp.y, p.x, p.y)
                        if dis < minn_dis:
                            dis = minn_dis
                            nearest = fp.rssi[now_fp.index] - now_fp.rssi
                        if dis < 200:
                            tmp.append(fp.rssi[now_fp.index])
                    if len(tmp) > 0:
                        d = float(sum(tmp))/len(tmp) - now_fp.rssi
                    else:
                        d = nearest
                p.w *= w_gauss(float(d) / FP_D, 0)
        """


        for p in particles:
            # map illegal
            if (p.x < stx) or (p.x > enx) or (p.y < sty) or (p.y > eny) or (p.w < EPSILON):
                p.w = 0
            for (x, y, lx, ly) in rect:
                if (p.x >= x) and (p.x <= x + lx) and (p.y >= y) and (p.y <= y + ly):
                    p.w = 0

        # ---------- Try to find current best estimate for display ----------
        m_x, m_y, m_confident = compute_mean_point(particles)

        # ---------- Show current state ----------
        plt.clf()
        if place == '11-211':
            plt.xlim(-500, 1000)
            plt.ylim(-1500, 2500)
        if place == 'corridor':
            plt.xlim(-1000, 5000)
            plt.ylim(-1000, 5000)
        if place == 'kejiyuan':
            plt.xlim(-200, 1500)
            plt.ylim(-200, 1500)
            plt.imshow(img)
        if place == 'office':
            plt.xlim(-100, 700)
            plt.ylim(-100, 1000)
            plt.imshow(img)
        plt.gca().invert_yaxis()
        plt.scatter([x for (x, y) in now_cv_points], [y for (x, y) in now_cv_points], marker='p',color='blue')
        groudTruth = [now_cv_points[0][0], now_cv_points[0][1]]
        plt.scatter([groudTruth[0]], [groudTruth[1]], marker='p', color='blue')
        plt.scatter([x for (x, y) in Beacons_locations], [y for (x, y) in Beacons_locations], marker='v')
        plt.scatter([p.x for p in particles if p.w > 0], [p.y for p in particles if p.w > 0], s=1, marker='x')
        plt.scatter([m_x], [m_y], marker='*')

        for (x, y, lx, ly) in rect:
            plt.gca().add_patch(plt.Rectangle((x, y), lx, ly))
        plt.arrow(m_x, m_y, math.cos(math.radians(now_stepYaw[1])) * stepLength,
                  math.sin(math.radians(now_stepYaw[1])) * stepLength, width=0.01)
        dx = 100 * math.cos(math.radians(now_stepYaw[1]))
        dy = 100 * math.sin(math.radians(now_stepYaw[1]))
        plt.arrow(0, 0, dx, dy, width=0.1)
        plt.ion()  #打开交互模式
        plt.plot([stx, stx, enx, enx, stx], [sty, eny, eny, sty, sty])
        plt.show() # 循环外
        plt.pause(0.01)
        plt.clf()  #清除图像
        plt.savefig('PF_result/particle_{}.png'.format(counter))
        # ---------- Mark Deviation -------------
        if place == '11-211':
            dx = float(m_x - groudTruth[0]) / (enx - stx) * 6.3
            dy = float(m_y - groudTruth[1]) / (eny - sty) * 11.5
        if place == 'cooridor':
            dx = float(m_x - groudTruth[0]) / (enx - stx) * 1.6
            dy = float(m_y - groudTruth[1]) / (eny - sty) * 21.6
        if place == 'office':
            dx = float(m_x - groudTruth[0]) / (enx - stx) * 12.0
            dy = float(m_y - groudTruth[1]) / (eny - sty) * 20.0
        #deviation.append(math.sqrt(dx * dx + dy * dy))
        deviation.append(m_confident)

        # mark the accuracy
        """
        dis0 = 0
        minDis = 0
        for i in range(len(now_cv_points)):
            dx = float(m_x - now_cv_points[i][0])
            dy = float(m_y - now_cv_points[i][1])
            dis = math.sqrt(dx * dx + dy * dy)
            if i == 0:
                dis0 = dis
                minDis = dis
            else:
                minDis = min(dis, minDis)

        if minDis == dis0:
            deviation.append(1)
        else:
            deviation.append(0)
        """


        # ---------- Shuffle particles ----------
        new_particles = []
        # Normalise weights
        nu = sum(p.w for p in particles)
        if nu:
            for p in particles:
                p.w = p.w / nu
        print('nu : {}\n'.format(nu))
        # create a weighted distribution, for fast picking
        dist = WeightedDistribution(particles)
        none_counter = 0
        for _ in particles:
            p = dist.pick()
            if p is None:  # or (random.uniform(0, 1) < 0.02):  # No pick b/c all totally improbable
                new_particle = Particle(x=random.uniform(stx, enx), y=random.uniform(sty, eny))
                none_counter += 1
            else:
                new_particle = Particle(p.x, p.y,
                                        heading=p.h,
                                        noisy=True)
            new_particles.append(new_particle)
        print('{} new particles\n'.format(none_counter))

        particles = new_particles

        # ---------- Move things ----------
        # Move particles according to my belief of movement (this may
        # be different than the real movement, but it's all I got)
        print('{} delta stepYaw\n'.format(d_h))
        for p in particles:
            #p.h += d_h + random.uniform(-5, 5)
            p.h = now_stepYaw[1] + random.uniform(-15, 15)
            p.advance_by(stepLength + random.uniform(-50, 100))

        # ---------- Next iter ----------
        now_timestamp += DELTA_TIMESTAMP
        counter += 1

        endTime = time.time()
        TimeConsuming.append(endTime - startTime)

    print(TimeConsuming)
    print('Time: ', sum(TimeConsuming) / len(TimeConsuming))
    return deviation


# ------------------------------------------------------------------------
def compute_mean_point(particles):
    """
    Compute the mean for all particles that have a reasonably good weight.
    This is not part of the particle filter algorithm but rather an
    addition to show the "best belief" for current position.
    """

    m_x, m_y, m_count = 0, 0, 0
    for p in particles:
        m_count += p.w
        m_x += p.x * p.w
        m_y += p.y * p.w

    if m_count == 0:
        return -1, -1, False

    m_x /= m_count
    m_y /= m_count

    # Now compute how good that mean is -- check how many particles
    # actually are in the immediate vicinity
    m_count = 0
    for p in particles:
        if distance(p.x, p.y, m_x, m_y) < 125:
            m_count += 1

    return m_x, m_y, float(m_count) / PARTICLE_COUNT
